#ifndef _DISCRETE_CIRCLE_SYSTEM_H_
#define	_DISCRETE_CIRCLE_SYSTEM_H_

#include <utility>
#include <vector>
#include <algorithm>
#include <functional>
#include <cmath>

#include <boost/math/constants/constants.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#ifdef _CILK_
#include <cilk/cilk.h>
#endif

#include <Geex/basics/types.h>

#include "common/particle_circle.h"
#include "common/containers.h"
#include "common/field_grid_2.h"
#include "common/circle_system.h"

//#include <worhp/worhp.h>

namespace Particle_packing
{
	using namespace Packing_common;
	using namespace boost::numeric::ublas;

	class Distance_field_grid_2 
	{
		typedef CGAL::Exact_predicates_inexact_constructions_kernel::Point_2 Point_2;
		typedef CGAL::Exact_predicates_inexact_constructions_kernel::Line_2	Line_2;
		typedef CGAL::Exact_predicates_inexact_constructions_kernel::Vector_2	Vector_2;
		typedef CGAL::Exact_predicates_inexact_constructions_kernel::Aff_transformation_2	Transformation_2;

	private:
		struct DistanceNode
		{
			std::vector<double> dist_polygon; // acctually squared distance up to now
			//  std::vector<Distance_field_grid_2::Point_2>	local_coords; // coordinates in the local frame of polygons
			std::vector<Vector_2> gradients; // the gradient of squared distance defined on this edge
			//std::vector<Point_2> closest_pnts; // to which edge this node is the closest for a polygon

			double x;
			double y;

			// for reconstructing
			inline void clear() 	{	dist_polygon.clear(); /*local_coords.clear();	*/ }
		};

		struct LocalFieldNode
		{
			double dist;
			Vector_2 grad;
			//Point_2 closest_pnt;
		};

	public:
		Distance_field_grid_2(const Container_box& container, size_t nb_rows, size_t nb_cols);
		~Distance_field_grid_2();

		// compute and store the distance field generated by one polygon
		void append_distance_field(const Particle_circle& circle) ;

		// after one polygon is transformed by "trans_param", update the correponding field
		void update_distance_field(size_t particle_id, const Transformation_2& transformation);

		// distance query in global grid
		double distance_query(const Point_2& p, size_t particle_id) const ;
		double gradient_query(const Point_2& p, size_t particle_id, Vector_2& grad/*, Point_2& closest_point*/) const;

		void clear() ;

		// draw
		// void gl_draw(size_t polygon_id) const; // draw the field generated by polygon[id]
		// void gl_draw_local_field(size_t polygon_id) const ; // time consuming, only for debug

	private:
		/* 
		** return the distance in a field grid using interpolation 
		** and gradient and closest point in the local coordinate
		*/
		double local_query(const Point_2& p, size_t particle_id, Vector_2& grad/*, Point_2& closest_point*/) const; 

	private:
		Grid_2<DistanceNode> grid;
		std::vector<Local_frame_2> local_frames; // local frames with the polygons
		std::vector< Grid_2<LocalFieldNode>* > local_fields; // the distance field expressed in the local coordinate system of each polygon
	};

	class Discrete_circle_system : public Circle_system
	{
		typedef CGAL::Exact_predicates_inexact_constructions_kernel::Aff_transformation_2	Transformation_2;

	public:
		Discrete_circle_system(const std::vector<Particle_circle>& particles, const Container_box& bndbox);

		int optimize(double *trans, bool apply_constraints = false);

		double gravity_energy_at(const double *const x) { }
		double molecular_energy_at(const double *const x);

		double system_energy_at(const double *const x) { return molecular_energy_at(x); }

		~Discrete_circle_system() { }

	private:
		void constraint_at(const double *const x, double *constraint_values);
		void constraint_grad_at(const double *const x, double *constraint_grad);
		void energy_gradient_at(const double *const x, double *gradient);

		void move_to_newpoint(const double *const x);

	private:
		double rm;

		std::vector< std::vector<Point_2> > sample_pnts;
		std::auto_ptr<Distance_field_grid_2> dfg;
		std::auto_ptr<Knitro_context> ktr_context;

		std::vector< matrix<double> > prev_trans;
	};

}

#endif